/*******************************************************************************
ApplicationPrototype.c

This is the module that is the most liquid for development of a new sensor
firmware prototype. In other words, this is where the "work" should be done
by the developer. The exception would be if changes need to be made to the 
communications handler or if a different vision chip is being used.

Origin:
Adapted from parts of VCinterface and AtmelInterface by Travis Young,

Change Log:
2010 May 21, Friday, birth
2010 August 9, Monday, documentation
2011 January 27, Thursday, added pause/resume and user-specified of_scale
******************************************************************************/

//#include "Headers.h"
#include "AVR8sa1os101.h"
#include <avr/io.h>
#include <avr/eeprom.h>
#include "F64PlusInterface.h"
#include "OFFunctions.h"
#include "ApplicationPrototype.h"
#include "I2CSupport.h"


//////////////////////////////////////////////////////////////////////////////
// LOAD_CAL_MASK: Loads the fixed pattern noise mask from EEPROM
//
// The fixed pattern noise mask is loaded from EEPROM to memory for usage
//
// Possible changes: None at the moment
//////////////////////////////////////////////////////////////////////////////
void load_cal_mask(void)
{
	for (unsigned int i=0; i<PINHOLE_IMAGE_SIZE; ++i) {
		unsigned char a = eeprom_read_byte((unsigned char*)EEP_cal_mask+i);
		Cal.read[i] = a;
	}
//	eeprom_read_block(&FPNMask,(unsigned char*)0x00,image_size*2);
		
}

//////////////////////////////////////////////////////////////////////////////
// STORE_CAL_MASK: Stores the fixed pattern noise mask to EEPROM
//
// This function writes the fixed pattern noise mask from memory to the
// EEPROM, for example after a new mask is computed.
//
// Possible changes: Use block write to be gentler to EEPROM? Right now every
// byte is written independently which is not good for the Atmel.
//////////////////////////////////////////////////////////////////////////////
void store_cal_mask(void) 
{	
	unsigned int i;
	for (i=0; i<PINHOLE_IMAGE_SIZE; ++i) {
		eeprom_update_byte((unsigned char*)EEP_cal_mask+i,Cal.read[i]);
	}
//	eeprom_write_block(&FPNMask,(unsigned char*)0x00,image_size*2);
}

//////////////////////////////////////////////////////////////////////////////
// LOAD_PINHOLE_LOCATION: Loads pinhole location from EEPROM
//
// Possible changes: None at the moment
//////////////////////////////////////////////////////////////////////////////
void load_pinhole_location(void) 
{
	unsigned char a = eeprom_read_byte((unsigned char*)EEP_pinhole_row);
	if (a>64)
		a=64;
	pinhole_row = a;
	a = eeprom_read_byte((unsigned char*)EEP_pinhole_col);
	if (a>64)
		a=64;
	pinhole_col = a;
	pinhole_location.read[0] = pinhole_row;
	pinhole_location.read[1] = pinhole_col;
}


//////////////////////////////////////////////////////////////////////////////
// SAVE_PINHOLE_LOCATION: Saves pinhole location to EEPROM
//
// Possible changes: Generally works fine
//////////////////////////////////////////////////////////////////////////////
void save_pinhole_location(void) 
{
	eeprom_update_byte((unsigned char*)EEP_pinhole_row,pinhole_row);
	eeprom_update_byte((unsigned char*)EEP_pinhole_col,pinhole_col);
//	eeprom_update_byte(4,25);
}


//////////////////////////////////////////////////////////////////////////////
// APPLICATION_INIT: Initialize application variables
//
// This function is called by main() and initializes variables. This will
// be one of the functions edited a lot by developers
//////////////////////////////////////////////////////////////////////////////
void Application_Init(void)  // DO NOT CHANGE NAME
{

	// INITIALIZE OUTPUT ARRAY VARIABLES
	// Note that output variables are constructed as a structure. Every one of
	// these variables needs to be initialized. Most important is to initialize
	// the read_length field. The locked field should also be initialized to
	// zero, though for the moment this field is not really used.
	// FPS variable
	FPS.locked = 0;		// unlock
	FPS.read_length = 2;
	// debug array variable
	debug.locked = 0;
	debug.read_length = 256;
	// pinhole location
	pinhole_location.locked = 0;
	pinhole_location.read_length = 3;

	//Image0 and Image1
	Image0.locked=0;
	Image0.read_length = MAX_PINHOLE_IMAGE_SIZE;
	Image1.locked=0;
	Image1.read_length = MAX_PINHOLE_IMAGE_SIZE;
	//ImageOut.locked=0;
	//ImageOut.read_length = MAX_PINHOLE_IMAGE_SIZE;
	
	// OFout0 and OFout1
	OFout0.locked=OFout1.locked=0;
	OFout0.read_length=OFout1.read_length = 6;
	pOFout = &OFout0;
	
	// raw_image_16x16
	raw_image_16x16.read_length = RAW_IMAGE_SIZE;
	raw_image_16x16.locked = 0;
	// OFrec
	OFrec.read_length = 100;
	OFrec.locked = 0;
	OFrec.read_index = 0;
	// Cal
	Cal.read_length = PINHOLE_IMAGE_SIZE;
	Cal.locked = 0;

	// INITIALIZE ENUMERATION VARIABLE
	// This is a ten element enumeration vector. This may be
	// used to help identify a sensor at startup. The following
	// format is suggested but not required.
	enumeration.read_length = 10;
	enumeration.locked = 0;
	enumeration.read[0] = 0x11; // suggested value for AVR8
	enumeration.read[1] = 0x00; // Hardware ID #2
	enumeration.read[2] = 0x00; // Sensor Head ID#1
	enumeration.read[3] = 0x00; // Sensor head ID#2
	enumeration.read[4] = 0x00; // Optics ID#
	enumeration.read[5] = 0x00; // Firmware ID#1
	enumeration.read[6] = 0x00; // Firmware ID#2
	enumeration.read[7] = 0x00; // Author ID#1
	enumeration.read[8] = 0x00; // Author ID#2
	enumeration.read[9] = 0x07; // Misc
	
	xmtbuf.comm_att = COMM_ATT_ENUM;  // 0 = enumeration
	xmtbuf.read = enumeration.read;
	xmtbuf.read_length = enumeration.read_length;
	xmtbuf.locked = &(enumeration.locked);

	// INITIALIZE OTHER VARIABLES
	control_flags = 0x00;

	which_Image_later = 0;
	pImageOut = &Image0;
	unsigned int i;
	for (i=0; i!=MAX_PINHOLE_IMAGE_SIZE; ++i) {
		Image0.read[i] = Image1.read[i] = 0;
		ImageLP[i] = 0;
	}

	LP8_skipcnt = 0; // reset lp8 skip counter

	// Load states from EEPROM - unset values will be 0xFF
	// Currently checks for valid ranges, make sure to modify if you
	//   add new functionality
	state_vars.read_length = 12;
	unsigned char a;
	a = eeprom_read_byte((unsigned char*)EEP_OF_MODE);
	OF_mode = ( a > 0 && a < 3 ? a : 1 );
	a = eeprom_read_byte((unsigned char*)EEP_USE_IMAGE_CAL);
	use_image_cal = ( a < 2 ? a : 0 );
	a = eeprom_read_byte((unsigned char*)EEP_GET_RAW_IMAGE);
	get_raw_image = ( a < 2 ? a : 0 );
	a = eeprom_read_byte((unsigned char*)EEP_OF_DEADZONE);
	OF_deadzone = ( a < 0xFF ? a : 1 );
	a = eeprom_read_byte((unsigned char*)EEP_HP_ALPHA);
	HPF_shift_amt = ( a < 0xFF ? a : 0 ); // 7
	a = eeprom_read_byte((unsigned char*)EEP_LP_OF);
	rof_shift = ( a < 0xFF ? a : 6 );
	a = eeprom_read_byte((unsigned char*)EEP_MIN_DISP);
	min_disp = ( a < 0xFF ? a : 5 );
	a = eeprom_read_byte((unsigned char*)EEP_ADDFPN_STRENGTH);
	AddFPN_strength = ( a < 0xFF ? a : 0 ); // 1
	a = eeprom_read_byte((unsigned char*)EEP_FRAME_SKIPPING);
	frames_to_skip = ( a < 0xFF ? a : 10 );
	a = eeprom_read_byte((unsigned char*)EEP_GET_FPS);
	get_FPS = ( a < 2 ? a : 0 );
	a = eeprom_read_byte((unsigned char*)EEP_LED_MODE);
	LED_mode = ( a < 0xFF ? a : 1 );
	a = eeprom_read_byte((unsigned char*)EEP_OF_SCALE);
	of_scale_byte = ( a < 0xFF ? a : 64 );
	of_scale_assign();

	frame_startup = 0;

	//Amp_Type = 0; // moved to VC_init();

	// For this version of the firmware, the pinhole location and the 
	// FPN calibration mask both need to be loaded.
	load_pinhole_location();
	load_cal_mask();

}

//////////////////////////////////////////////////////////////////////////////
// PROCESS_APPLICATION_COMMAND: Process an application command.
//
// This function is called by process_commands(). All application firmware
// specific commands should be handled here. This includes all commands
// with a header greater than or equal to decimal 60. Variable cmd0 is 
// the header, while cmd1 is the argument.
//
// Possible edits: Every time a new command is created.
//////////////////////////////////////////////////////////////////////////////
// DO NOT CHANGE THE NAME OF THIS FUNCTION
void process_application_command(unsigned char cmd0, unsigned char cmd1)
{
	switch (cmd0) {
		// THESE ARE APPLICATION COMMANDS

		//change the current OF mode
		case COMM_CMD_OF_MODE:	
			if ( cmd1==1 || cmd1==2 ) {
				OF_mode = cmd1;
				eeprom_update_byte((unsigned char*)EEP_OF_MODE, cmd1);
			}
			break;

	    //find the calibration mask. should be called only after the pinhole
		// is found.
		case COMM_CMD_IMAGE_CAL_ENABLE:	
			if(cmd1==0x55) {
				find_pinhole_fpn_calibration();
				store_cal_mask();
//				change_mask=1;
//				FPNMaskEnable_temp=cmd1;
			}
			break;

		// use the FPN calibration mask
		case COMM_CMD_USE_IMAGE_CAL:
			if ( cmd1==0 || cmd1==1 ) {
				use_image_cal = cmd1;
				eeprom_update_byte((unsigned char*)EEP_USE_IMAGE_CAL, cmd1);
			}
//			if(cmd1==1)
//				FPNCalibrate=OM_FPN_CAL_EXEC;
			break;

		//enable DRC/reset DRC
//		case COMM_CMD_DRC_ENABLE:
//			break;


		//change amount of frames in between OF calculations. This may be
		//useful when the optical flow is very slow and we thus need to 
		//change the effective frame rate.
		case COMM_CMD_FRAME_SKIPPING:
			frames_to_skip=cmd1;
			eeprom_update_byte((unsigned char*)EEP_FRAME_SKIPPING, cmd1);
			break;

		//finds the pinhole location. Once the optics are mounted, one would
		// aim the sensor towards a dominantly bright light (with no other
		// bright lights visible by the sensor) and select this command. The
		// pinhole location will simply be the brightest spot. The pinhole
		// location is then stored in EEPROM
		case COMM_CMD_FIND_PINHOLE:
			if (cmd1==0x55) {
				pinhole_find();
				save_pinhole_location();
			}
			break;

		// This command allows a user to manually set the pinhole row location,
		// for example to adjust it after an automatic set.
		case COMM_CMD_SETROW_PINHOLE:
			if ((cmd1&0xC0) == 0x40) {
				pinhole_row = cmd1 & 0x3F;
				pinhole_location.read[0] = pinhole_row;
				save_pinhole_location();
			}
			break;

		// This command allows a user to manually set the pinhole column location.
		case COMM_CMD_SETCOL_PINHOLE:
			if ((cmd1&0xC0) == 0x40) {
				pinhole_col = cmd1 & 0x3F;
				pinhole_location.read[1] = pinhole_col;
				save_pinhole_location();
			}
			break;

		// This command changes the "alpha" amount used for the high-pass filter
		case COMM_CMD_HP_ALPHA:
			HPF_shift_amt = cmd1;
			eeprom_update_byte((unsigned char*)EEP_HP_ALPHA, cmd1);
			break;

		// This command changes the added FPN mask. Note that this is different
		// from the FPN mask subtracted out.
		case COMM_CMD_ADDFPN_STRENGTH:
			AddFPN_strength = cmd1;
			eeprom_update_byte((unsigned char*)EEP_ADDFPN_STRENGTH, cmd1);
			break;

		// This command changes the low pass filter update rate used for 
		// optical flow computations
		case COMM_CMD_LP_OF:
			rof_shift = cmd1;
			eeprom_update_byte((unsigned char*)EEP_LP_OF, cmd1);
			break;

		// This command changes whether or not to acquire the raw image. The raw
		// image is different from the pinhole image- it is every 4th row and column
		// of the raw array, whether lit up by the pinhole or not, and is usedful
		// for diagnostic purposes. Enabling it is not necessary for current optical
		// flow algorithms, and enabling it slows down the sensor operation.
		case COMM_CMD_GET_RAW_IMAGE:
			if ( cmd1==0 || cmd1==1 ) {
				get_raw_image = cmd1;
				eeprom_update_byte((unsigned char*)EEP_GET_RAW_IMAGE, cmd1);
			}
			break;

		// This is the "dead zone" used to determine whether an optical flow value
		// is added to the accumulation for odometry. This is a cheap hack to help
		// reduce drift when the sensor is still, but can prevent odometry operation
		// when the optical flow is very slow.
		case COMM_CMD_OF_DEADZONE:
			OF_deadzone = cmd1;
			eeprom_update_byte((unsigned char*)EEP_OF_DEADZONE, cmd1);
			break;

		// This is the "minimum displacement" for when the optical flow method is 2.
		case COMM_CMD_MIN_DISP:
			min_disp = cmd1;
			eeprom_update_byte((unsigned char*)EEP_MIN_DISP, cmd1);
			break;

		// Option to compute frame rate (FPS)
		case COMM_CMD_GET_FPS:	
			if ( cmd1==0 || cmd1==1 ) {
				get_FPS = cmd1;
				eeprom_update_byte((unsigned char*)EEP_GET_FPS, cmd1);
			}
			break;

		//change the current LED mode
		case COMM_CMD_LED_MODE:	
			if ( cmd1==1 || cmd1==2 ) {
				LED_mode = cmd1;
				eeprom_update_byte((unsigned char*)EEP_LED_MODE, cmd1);
			}
			PORTA &= 0xEF; // reset to off
			break;

		// Change the scaling of the output. Default is 64*2
		// This currently doubles the value of cmd1 to provide a range of 0-508 in even numbers,
		// though this may change based on needs
		case COMM_CMD_OF_SCALE:
			// TODO: What do we do when the desired value is outside the range of 0-508?
			//of_scale = ((float) cmd1)*2;
			of_scale_byte = cmd1;
			of_scale_assign();
			eeprom_update_byte((unsigned char*)EEP_OF_SCALE, cmd1);
			break;

		// Pause everything except i/o and command processing
		// Mainly for debugging and making sure that the image you read is from one frame
		// KEYED, send the same number as the argument to use.
		case COMM_CMD_PAUSE:
			if(cmd1 == COMM_CMD_PAUSE){
				control_flags |= 0x01;
				PORTA &= 0xEF; // flip LED off
			}
			break;

		// Resume everything
		case COMM_CMD_RESUME:
			control_flags &= 0xFE;
			break;

	} // switch
}

//////////////////////////////////////////////////////////////////////////////
// PROCESS_APPLICATION_TRANSMIT: Process an application command.
//
// This function is called by process_transmit(). When the OS command to change
// the ATT (array to transmit) is sent, this function changes the output buffer
// so that it points to the appropriat output structure. As new output variables
// are added, they should be added in a way to follow the format below.
//
// Possible edits: Every time a new application level output array is created
//////////////////////////////////////////////////////////////////////////////
// DO NOT CHANGE THE NAME OF THIS FUNCTION
void process_application_transmit(unsigned char array_to_transmit)
{

	switch (array_to_transmit) {
		// Returns FPS as a two-byte number
		case COMM_ATT_FPS:
			xmtbuf.comm_att = COMM_ATT_FPS;
			xmtbuf.read = FPS.read;
			xmtbuf.read_length = FPS.read_length;
			xmtbuf.locked = &(FPS.locked);
			break;

		// Returns DRC min and max- currently not used since DRC is not used.
		case COMM_ATT_DRC:
//			if ((transmit_counter) == 0)
//				byte = 255-((Max_temp&0x0FF0)>>4);;
//			if ((transmit_counter) == 1)
//				byte = 255-((Min_temp&0x0FF0)>>4);
			break;

		//returns raw pixels
		case COMM_ATT_RAW:
			xmtbuf.comm_att = COMM_ATT_RAW;
			xmtbuf.read = raw_image_16x16.read;
			xmtbuf.read_length = raw_image_16x16.read_length;
			xmtbuf.locked = &(raw_image_16x16.locked);
			break;

		// Returns the optical flow array
		case COMM_ATT_OFOUT1:
			xmtbuf.comm_att = COMM_ATT_OFOUT1;
			
			xmtbuf.read = OFout1.read;
			xmtbuf.read_length = OFout1.read_length;
			xmtbuf.locked = &(OFout1.locked);

			//xmtbuf.read = pOFout->read;
			//xmtbuf.read_length = pOFout->read_length;
			//xmtbuf.locked = &(pOFout->locked);

			break;

		// Returns the pinhole location
		case COMM_ATT_PINHOLE_LOC:
			xmtbuf.comm_att = COMM_ATT_PINHOLE_LOC;
			xmtbuf.read = pinhole_location.read;
			xmtbuf.read_length = pinhole_location.read_length;
			xmtbuf.locked = &(pinhole_location.locked);
			break;

		// Returns the pinhole image
		case COMM_ATT_IMAGE_OUT:
			xmtbuf.comm_att = COMM_ATT_IMAGE_OUT;

			xmtbuf.read = pImageOut->read;
			//xmtbuf.read_length = pImageOut->read_length; // This set it to the max size, not actual
			xmtbuf.read_length = PINHOLE_IMAGE_SIZE;
			xmtbuf.locked = &(pImageOut->locked);
			
			//xmtbuf.read = ImageOut.read;
			//xmtbuf.read_length = ImageOut.read_length;
			//xmtbuf.locked = &(ImageOut.locked);
			break;

		// Returns the last command executed
		case COMM_ATT_LAST_CMD:
			xmtbuf.comm_att = COMM_ATT_LAST_CMD;
			xmtbuf.read = last_cmd.read;
			xmtbuf.read_length = last_cmd.read_length;
			xmtbuf.locked = &(last_cmd.locked);
			break;

		// Returns the debug array, for diagnostics
		case COMM_ATT_DEBUG:
			xmtbuf.comm_att = COMM_ATT_DEBUG;
			xmtbuf.read = debug.read;
			xmtbuf.read_length = debug.read_length;
			xmtbuf.locked = &(debug.locked);
			break;

		// Returns the optical flow record array
		case COMM_ATT_OFREC:
			xmtbuf.comm_att = COMM_ATT_OFREC;
			xmtbuf.read = OFrec.read;
			xmtbuf.read_length = OFrec.read_length;
			xmtbuf.locked = &(OFrec.locked);
			break;

		// Returns the calibration mask
		case COMM_ATT_CAL:
			xmtbuf.comm_att = COMM_ATT_CAL;
			xmtbuf.read = Cal.read;
			xmtbuf.read_length = Cal.read_length;
			xmtbuf.locked = &(Cal.locked);
			break;

		// Persistent state variables
		case COMM_ATT_STATES:
			xmtbuf.comm_att = COMM_ATT_STATES;
			xmtbuf.read = state_vars.read;
			xmtbuf.read_length = state_vars.read_length;
			xmtbuf.locked = &(state_vars.locked);
			break;
	}

}

//////////////////////////////////////////////////////////////////////////////
// APPLICATION_MAINLOOP: This is the primary loop run by the application
//
// Formerly "DataInterchange"
//
// This function is called by main(). This is the main loop performed by the
// application. The general "run time" vision algorithms are called here.
//
// Possible edits: The contents of function "Geof sequence" can be copied in here
//////////////////////////////////////////////////////////////////////////////
void Application_MainLoop(void) // DO NOT CHANGE NAME
{

	//start timer
	TCNT1=0;

	// NOTE: We need at least one call to "process_commands" in this loop
	// This is what causes all pending commands to be processed. If this
	// loop will be slow or if there will be a lot of commands being 
	// written to the device, then it may be helpful to call this command
	// several times within this mainloop function.
	process_commands(); 

	if(control_flags & 0x01)
		return; // Computation and updates paused

	// set biases if required
	if (BiasSettings.UpdateReq)
		set_biases();

	process_images();

	process_commands();

	//compute frame elapsed time
	if(get_FPS) {
		float end_time=(float)TCNT1;
		FPS.FPS=(float)(7812/end_time);
		if (FPS.locked==0) { // update read array if unlocked
			FPS.read[0] = ((unsigned short int)FPS.FPS >> 8) & 0xFF;
			FPS.read[1] = ((unsigned short int)FPS.FPS) & 0xFF;		
		}
	}

	// hack for testing i2c read
//	enumeration.read[3]++;

}

//////////////////////////////////////////////////////////////////////////////
// PROCESS_IMAGES: Doin' stuff... The contents of this function can be copied
// into the application main loop above. I am just too lazy to do so at the
// moment...
//////////////////////////////////////////////////////////////////////////////
void process_images(void)
{
	//unsigned char i,j; // no longer used apparently
	unsigned int ii;

debug.read[0]++;

	// grab a raw 16x16 image (every 4th row/col) if needed
	// note that this slows down execution
	if (get_raw_image)
		get_raw_image_16x16();
	
	// Counter for the 8-bit version of the high-pass filter (currently not used)
	if (LP8_skipcnt)
		LP8_skipcnt --;
	else
		LP8_skipcnt = 10;

	// frame counter- enables skipping of frames for OF processing
	if (frame_count)
		frame_count--;
	else
		frame_count = frames_to_skip;

	// Here we begin the main optical flow processing
	unsigned char *pIcurr, *pIprev;
	struct ImageBuf *pImageBufcurr;

	if (OF_mode == 1) { 
		// This is the original mode- compute OF, accumulation, and running avg
		if (frame_count==0) {
			if(of_count < 255)
				++of_count;
			else
				of_count = 0;

			// Switch between Image0 and Image1 on every 
			// frame so that one frame Image0 is newer, and the other Image1 is newer
			// Below: 	Pix_HPF is main high pass filter
			//			AddFPN adds fixed pattern noise (to prevent zero condition)
			//			IIA compute OF using image interpolation
			if (which_Image_later==1) {
				which_Image_later = 0;  // setting for the next iteration of process_images
				pIcurr = Image1.read;
				pIprev = Image0.read;
				pImageBufcurr = &Image1;
			} else {
				which_Image_later = 1;  // setting for the next iteration of process_images
				pIcurr = Image0.read;
				pIprev = Image1.read;
				pImageBufcurr = &Image0;
			}
			
			// grab the image from the pinhole for OF processing
			get_pinhole_image(pIcurr);
			/*
			if(which_Image_later==1) {
				for(i=0;i<PINHOLE_IMAGE_SIZE;i++)
					Image0.read[i]=of_count;
			} else {
				for(i=0;i<PINHOLE_IMAGE_SIZE;i++)
					Image1.read[i]=of_count;
			}
			*/	

			// process the most recently grabbed image
			SubtractCalMask(pIcurr);
			Pix_HPF(pIcurr, ImageLP, HPF_shift_amt);				
			AddFPN(pIcurr,AddFPN_strength);

			// Now that the current image has been grabbed and completely processed
			// we can set it as the output image
			// This essentially implements double buffering when combined with the code above
			// Note that if the frame rate is fast (i.e. frames_to_skip==0 and rof_shift==0), 
			// these arrays may get overwritten because this function loops through twice
			// This could be solved by using multiple buffering (i.e. quad buffering), but that
			// would be a waste of memory. If you are really interested in the output image,
			// then set fremes_to_skip to a high value (i.e. >200)
			pImageOut=pImageBufcurr;
			//for(i=0;i<PINHOLE_IMAGE_SIZE;++i)
			//	ImageOut.read[i]=pIcurr[i];

			// Perform calculation of optic flow
			IIA2(pIprev,pIcurr);

			// Store optical flow in OFrec array- this is mostly for diagnostics
			OFrec.read[OFrec.read_index++] = OFX;
			OFrec.read[OFrec.read_index++] = OFY;
			if (OFrec.read_index >= 99)
				OFrec.read_index = 0;

			// cumulative optical flow measurement e.g. odometry by simple sums
			signed int sig1 = OFX;
			signed int sig2 = OFY;
			if (sig1<0)
				sig1 = -sig1;
			if (sig2<0)
				sig2 = -sig2;
			// Note that we only add to the accumulation if the current optical
			// flow measurement exceeds the deadzone amount- this is a hack to
			// help limit drift when still. This can be disabled by setting the
			// deadzone to zero.
			if (sig1>=OF_deadzone || sig2>=OF_deadzone) {
				CX = CX + (signed int)OFX;
				CY = CY + (signed int)OFY;
				if (CX > (16*256))
					CX = CX - (32*256);
				if (CX < (-16*256))
					CX = CX + (32*256);
				if (CY > (16*256))
					CY = CY - (32*256);
				if (CY < (-16*256))
					CY = CY + (32*256);
			}

			// Low-pass filtered optic flow calculations
			if(rof_shift==0) {
				ROFX = OFX;
				ROFY = OFY;
			} else {
				float fac = (float)1 / (2^rof_shift);
				ROFX += ((float)OFX - ROFX) * fac;
				ROFY += ((float)OFY - ROFY) * fac;
			}

			//------------------------------------------------------------------
			// Store the resulting values to the OF output array
			//
			// Output is double buffered
			//
			if(which_Image_later==0) {
				OFout0.read[0] = (unsigned char)OFX;
				OFout0.read[1] = (unsigned char)OFY;
				OFout0.read[2] = (unsigned char)((CX>>5) & 0xFF);
				OFout0.read[3] = (unsigned char)((CY>>5) & 0xFF);
				OFout0.read[4] = (unsigned char)(ROFX);
				OFout0.read[5] = (unsigned char)(ROFY);
				pOFout = &OFout0;  // now that buffer is filled, I can set this as output buffer
			} else {
				OFout1.read[0] = (unsigned char)OFX;
				OFout1.read[1] = (unsigned char)OFY;
				OFout1.read[2] = (unsigned char)((CX>>5) & 0xFF);
				OFout1.read[3] = (unsigned char)((CY>>5) & 0xFF);
				OFout1.read[4] = (unsigned char)(ROFX);
				OFout1.read[5] = (unsigned char)(ROFY);				
				pOFout = &OFout1;  // now that buffer is filled, I can set this as output buffer
			}
			//------------------------------------------------------------------
	
		} // frame_count == 0
	} else if (OF_mode == 2) {
		
		// This is the straight odometry with updating every time the
		// displacement exceeds a threshold
		//signed char oldofx,oldofy; // unused?
		// High pass and send result to ImageX1
		get_pinhole_image(Image1.read);
		//Pix_HPF(Pinhole_image,ImageLP,ImageX1,HPF_shift_amt,64);
		Pix_HPF(Image1.read, ImageLP, HPF_shift_amt);
		// Add fixed-pattern noise to prevent zero condition
		AddFPN(Image1.read,AddFPN_strength);
		// Copy X1 result to image output array
		pImageOut=&Image1;
		//for(i=0;i<PINHOLE_IMAGE_SIZE;++i)
		//{
		//	ImageOut.read[i]=Image1.read[i];
		//}

		// Now do the computations
		if (frame_startup==0) {
//debug.read[1]++; // for debugging
			frame_startup=1;
			// We are beginning- we need to reset the measurements
			OFACCX=0;
			OFACCY=0;
			ODOX=0;
			ODOY=0;
			OFX=0;
			OFY=0;
			// copy Image1 to Image0
			for (ii=0; ii<PINHOLE_IMAGE_SIZE; ++ii)
				Image0.read[ii] = Image1.read[ii];
		} else {
//debug.read[2]++; for debugging
			// regular operation e.g. not at beginning- compute displacement
			// This is because X0 and X1 will likely be different
			// resulting optical flow stored in OFX and OFY.
			IIA2(Image0.read,Image1.read);
			// see if displacement > threshold
			float aofx,aofy;
			if (OFX>=0)
				aofx=OFX;
			else
				aofx=-OFX;	
			if (OFY>=0)
				aofy=OFY;
			else
				aofy=-OFY;

			if (aofx>=min_disp || aofy>=min_disp) {
				// if we get here, displacement > threshold and we update!
//debug.read[3]++;
				// Add to accumulation
				OFACCX += OFX;
				OFACCY += OFY;
				// roll over accumulation if needed
				if (OFACCX > (8*256))
					OFACCX = OFACCX - (16*256);
				if (OFACCX < (-8*256))
					OFACCX = OFACCX + (16*256);
				if (OFACCY > (8*256))
					OFACCY = OFACCY - (16*256);
				if (OFACCY < (-8*256))
					OFACCY = OFACCY + (16*256);
				// zero out OFX and OFY since they were added to cumulation
				OFX=0;
				OFY=0;
				// copy over X1 to X0 since contents of X1 form new base.
				for (ii=0; ii<PINHOLE_IMAGE_SIZE; ++ii)
					Image0.read[ii] = Image1.read[ii];
			}
		}
		// Now generate output = accumulation plus optical flow
		ODOX = OFACCX + OFX;
		ODOY = OFACCY + OFY;
		// roll over if necessary
		if (ODOX > (8*256))
			ODOX = ODOX - (16*256);
		if (ODOX < (-8*256))
			ODOX = ODOX + (16*256);
		if (ODOY > (8*256))
			ODOY = ODOY - (16*256);
		if (ODOY < (-8*256))
			ODOY = ODOY + (16*256);			
		// send output to optical flow output array
		OFout1.read[0] = (unsigned char)(ODOX/16);
		OFout1.read[1] = (unsigned char)(ODOY/16);
		OFout1.read[2] = 0;//(unsigned char)(OFACCX);
		OFout1.read[3] = 0;//(unsigned char)(OFACCY);
		OFout1.read[4] = 0;
		OFout1.read[5] = 0;

	}

	//-------------------------------------------------------------------------
	// LED function
	//
	if(LED_mode == 1) {
		// Illuminate the LED when x optical flow is positive
		if (OFX>0)
			PORTA |= 0x10;
		else
			PORTA &= 0xEF;
	} else if(LED_mode == 2) {
		// Change the LED state every 256 optic flow computations
		// This is primarily used to benchmark the computation of optical flow
		// The LED will hang if frames_to_skip is an odd number (not sure why right now).
		if(of_count==0)
			PORTA ^= 0x10;
		//if(of_count == 0) {
		//	if(PORTA & 0x10)
		//		PORTA &= 0xEF;
		//	else
		//		PORTA |= 0x10;
		//}
	
	}
	//-------------------------------------------------------------------------

	/*
	DDRC |= 0x10;
	DDRB |= 0x10;
	if (OFX>0) {
		PORTC |= 0x10;
		PORTB |= 0x10;
	} else {
		PORTB &= 0xEF;
		PORTC &= 0xEF;
	}
	*/
}


//////////////////////////////////////////////////////////////////////////////
// PINHOLE_FIND: Find the location of the pinhole
//
// This algorithm finds the brightest pixel inside the array, neglecting
// the outer ring of ten pixels, and determines that to be the location
// of the pinhole. This algorithm operates using raw e.g. unamplified pixels
// which should be adequate for most applications.
//////////////////////////////////////////////////////////////////////////////
void pinhole_find(void)
{
	unsigned char i,j,pixval,flag;

	// setup stuff
	LoadCmd((VCF64_CMD_MIDCONFIG+0x0C));	// raw readout unity gain using shortsc
	LoadCmd((VCF64_CMD_MIDOP+0x01));		// turn off amp
	LoadCmd((VCF64_CMD_VSW));				// no binning
	LoadCmd((VCF64_CMD_HSW));
	LoadCmd((VCF64_CMD_MISC+0x16));		// raw readout
	LoadCmd((VCF64_CMD_ADCOP+0x06));		// prepare ADC


	// Find brightest pixel
	flag = 1;
	for (i=10; i!=55; ++i) {
		LoadCmd((VCF64_CMD_ROWSEL+i));
		for (j=10; j!=55; ++j) {
			GetPixADC(pixval,j);
			if (pixval<pinhole_val || flag) {
				pinhole_val = pixval;
				pinhole_row = i;
				pinhole_col = j;
				flag=0;
			}
		}
	}

	// Store in readout structure - forego lock check
	pinhole_location.read[0] = pinhole_row;
	pinhole_location.read[1] = pinhole_col;
	pinhole_location.read[2] = pinhole_val;

	// Write resulting values to debug array
//	debug.read[248] = pinhole_row;
//	debug.read[249] = pinhole_col;
//	debug.read[250] = pinhole_val;

//	debug.read[0] += 10;
}


//////////////////////////////////////////////////////////////////////////////
// GET_PINHOLE_IMAGE: Find the location of the pinhole
//
// Grab an 8x8 image about the pinhole and store in the pinhole array. Originally
// this was written for a 16x16 array, but it was shortened here.
//
// Future changes may make this able to select among different image
// sizes rather than one fixed size.
//////////////////////////////////////////////////////////////////////////////
void get_pinhole_image(unsigned char *const I)
{
	unsigned char i,j;
	unsigned char *pI=I;

	// setup stuff
	LoadCmd((VCF64_CMD_MIDCONFIG+0x0C));	// raw readout unity gain using shortsc
	LoadCmd((VCF64_CMD_MIDOP+0x01));		// turn off amp
	LoadCmd((VCF64_CMD_VSW));				// no binning
	LoadCmd((VCF64_CMD_HSW));
	LoadCmd((VCF64_CMD_MISC+0x16));			// raw readout
	LoadCmd((VCF64_CMD_ADCOP+0x06));		// prepare ADC

	for (i=pinhole_row-PINHOLE_IMAGE_HEIGHT/2; i!=pinhole_row+PINHOLE_IMAGE_HEIGHT/2; ++i) {
		LoadCmd((VCF64_CMD_ROWSEL+i));
		amplify();
		for (j=pinhole_col-PINHOLE_IMAGE_WIDTH/2; j!=pinhole_col+PINHOLE_IMAGE_WIDTH/2; ++j) {
			GetPixADC(*(pI++),j);
		}
	}
}


//////////////////////////////////////////////////////////////////////////////
// FIND_PINHOLE_FPN_CALIBRATION: Find the location of the pinhole
//
// Find the brightest pixel in the center region of the raw 64x64 array
// and define these to be the row and column of the pinhole. These values
// are then stored in EEPROM. Any new pinhole images will be centered about
// the found pixel location.
//
// Generally called when an appropriate command is issued to the sensor
//////////////////////////////////////////////////////////////////////////////
void find_pinhole_fpn_calibration(void)
{
	unsigned int i;
	unsigned int CalSum[PINHOLE_IMAGE_SIZE];
	unsigned char count;
	unsigned char minmaskval;

	// zero out sum
	for (i=0; i<PINHOLE_IMAGE_SIZE; ++i)
		CalSum[i]=0;

	// grab image 64 times
	for (count=0; count<64; ++count) {
		get_pinhole_image(Cal.read);
		for (i=0; i<PINHOLE_IMAGE_SIZE; ++i) 
			CalSum[i] += Cal.read[i];
	}

	// compute mask - first pass
	minmaskval = 255;
	for (i=0; i<PINHOLE_IMAGE_SIZE; ++i) {
		Cal.read[i] = CalSum[i]/64;
		if (Cal.read[i]<minmaskval)
			minmaskval = Cal.read[i];
	}

	// compute mask - subtract out min
	for (i=0; i<PINHOLE_IMAGE_SIZE; ++i)
		Cal.read[i] -= minmaskval;
}

//////////////////////////////////////////////////////////////////////////////
// GET_RAW_IMAGE_16X16: Gets a 16x16 image
//
// Grabs a 16x16 raw image by skipping every four rows/cols from 64x64
// Store the result in raw_image_16x16. The former is the
// working array while the latter is for output generation
//
// work in progress
//////////////////////////////////////////////////////////////////////////////
void get_raw_image_16x16(void)
{
	unsigned char i,j,pixval;
	unsigned int ndx;

	// setup stuff
	LoadCmd((VCF64_CMD_MIDCONFIG+0x0C));	// raw readout unity gain using shortsc
	LoadCmd((VCF64_CMD_MIDOP+0x01));		// turn off amp
	LoadCmd((VCF64_CMD_VSW));				// no binning
	LoadCmd((VCF64_CMD_HSW));
	LoadCmd((VCF64_CMD_MISC+0x16));			// raw readout
	LoadCmd((VCF64_CMD_ADCOP+0x06));		// prepare ADC

	ndx=0;
	for (i=1; i<64; i+=4) {
		LoadCmd((VCF64_CMD_ROWSEL+i));
		amplify();
		for (j=1; j<64; j+=4) {
			GetPixADC(pixval,j);
			raw_image_16x16.read[ndx] = pixval;
			ndx++;
		}
	}

}
